#ifndef __FPSCAMERA_H__
#define __FPSCAMERA_H__

#include "Matrix4.h"
#include "Vector3.h"
#include "Debug.h"
#include "GLError.h"

class FPSCamera{
 public:
 FPSCamera(Vector3 position, Vector3 up, Vector3 lookAt) : position(position), up(up), speed(1.0), lookAt(lookAt) { 
		recalc_front();

		right = Vector3::cross(front, up);
		right.normalize();
	}

	void Y(double UP) {
		position.v[1] += UP;
		lookAt.v[1] += UP;

		recalc_front();
	}
	
	Vector3 recalc_front() {
		front = (lookAt-position);
		front.normalize();
		return front;
	}

	void forward(double amount) { 
		Vector3 delta = front*speed*amount;
		position = position + delta;		
		lookAt = lookAt + delta;
		recalc_front();
	}

	void backward(double amount) { forward(-amount); }

	void strafe_right(double amount) { 
		position=position+right*speed*amount; 
		lookAt = lookAt+right*speed*amount;
		recalc_front();
	}
	void strafe_left(double amount) { strafe_right(-amount); }

	void look_upward(Radians ang) { look_downward(-ang.radians); }

	void look_downward(Radians ang) { 
		lookAt = Matrix4::createTranslationMatrix(position)
			* Matrix4::createRotationMatrix(right, -ang.radians)
			* Matrix4::createTranslationMatrix(position*(-1.0))
			* lookAt;
		
		front = Matrix4::createRotationMatrix(right, ang.radians)*front;
		front.normalize();
	}
	
	void setSpeed(double newSpeed) {
		speed = newSpeed;
	}

	void rotate_leftward(Radians ang) { 
		lookAt = Matrix4::createTranslationMatrix(position)
			* Matrix4::createRotationMatrix(Vector3(0,1,0), ang.radians)
			* Matrix4::createTranslationMatrix(position*(-1.0))
			* lookAt;

		recalc_front();
		right  = Matrix4::createRotationMatrix(Vector3(0,1,0), ang.radians)*right;
		right.normalize();
	}
	void rotate_rightward(Radians ang) { rotate_leftward(Radians(-ang.radians)); }

	void fly_up(double amount) {
		position=position+Vector3(0,1,0)*speed*amount; 
		lookAt=lookAt+Vector3(0,1,0)*speed*amount;
	}

	void fly_down(double amount) {
		fly_up(-amount);
	}

	void debug() {
		position.print("position:");
		front.print("front:");
		up.print("up:");
		lookAt.print("lookAt:");
	}

	void load() {
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		gluLookAt(position[0], position[1], position[2], lookAt[0], lookAt[1], lookAt[2], up[0], up[1], up[2]);
		errorcheck(__FILE__,__LINE__);
	}

	void parse_key(unsigned char key) {
		switch(key) {
		case 'w':
			forward(1.0);
			break;
		case 'a':
			strafe_left(1.0);
			break;
		case 'd':
			strafe_right(1.0);
			break;
		case 's':
			backward(1.0);
			break;
		case 'q':
			rotate_leftward(Radians(0.05));
			debug();
			break;
		case 'e':
			rotate_rightward(Radians(0.05));
			debug();
			break;
		case ' ':
			fly_up(1.0);
			break;
		case 'z':
			fly_down(1.0);
			break;
		case 'i':
			look_upward(Radians(0.05));
			break;
		case 'j':
			look_downward(Radians(0.05));
			break;
		}
	}

	Vector3 position;
	Vector3 up,right,front;
	Vector3 lookAt;
	double speed;
};

#endif
